﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace UltrasonicTDPlatform
{
    class MyAlgorithm
    {
        public static double[] MethodTra(double[]real, double[]imag, double[] frequency)
        {
/*            // 1.导纳实部
            double[] real = new double[impandence.Length];
            // 2. 导纳虚部
            double[] imag = new double[impandence.Length];*/

            /*for(int i = 0; i < real.Length; i++)
            {
                real[i] = impandence[i] * Math.Cos(phase[i] * Math.PI / 180);
                imag[i] = impandence[i] * Math.Sin(phase[i] * Math.PI / 180);
            }*/
            int fsIdx = 0, f1Idx = 0, f2Idx = 0;
            // 3. 搜索实部极大值点fs，除以2就是电阻R1倒数，频率就是fs
            for(int i = 0; i < real.Length; i++)
            {
                if(real[i] >= real[fsIdx])
                {
                    fsIdx = i;
                }
            }

            // 4. 搜索虚部极大值和极小值，分别是f1和f2,这里直接找导纳实部接近最大值一半的频率点
            for(int i = 0; i < fsIdx; i++)
            {
                if(real[i] <= real[fsIdx]/ 2 && real[i + 1] >= real[fsIdx] / 2)
                {
                    f1Idx = i;
                    break;
                }
            }
            for (int i = fsIdx; i < real.Length-1; i++)
            {
                if (real[i] >= real[fsIdx] / 2 && real[i + 1] <= real[fsIdx] / 2)
                {
                    f2Idx = i;
                    break;
                }
            }

            // 5. 根据公式，把等效参数算出来
            double f1 = frequency[f1Idx];
            double f2 = frequency[f2Idx];
            double fs = frequency[fsIdx];

            double r = real[fsIdx] / 2;
            double yc = imag[fsIdx];

            double pi = Math.PI;
            double C0 = yc / (2 * pi * fs)*1000000000;
            double R1 = 1 / (2 * r);
            double C1 = 2 * pi * (f2 - f1) / (2 * pi * f1 * 2 * pi * f2 * R1)*1000000000000;
            double L1 = R1 / (2 * pi * (f2 - f1));

            return new double[] {C0,R1,C1,L1,f1,f2,fs};
        }   
    }

    class FFT
    {
        int n, m;
        // Lookup tables. Only need to recompute when size of FFT changes.
        double[] cos;
        double[] sin;

        public FFT(int n)
        {
            this.n = n;
            this.m = (int)(Math.Log(n) / Math.Log(2));

            // Make sure n is a power of 2
            if (n != (1 << m))
                Console.Out.WriteLine("FFT length must be power of 2");

            // precompute tables
            cos = new double[n / 2];
            sin = new double[n / 2];

            for (int i = 0; i < n / 2; i++)
            {
                cos[i] = Math.Cos(-2 * Math.PI * i / n);
                sin[i] = Math.Sin(-2 * Math.PI * i / n);
            }

        }
        ///x为实部y为虚部///
        public void fft(double[] x, double[] y)
        {
            int i, j, k, n1, n2, a;
            double c, s, t1, t2;

            // Bit-reverse
            j = 0;
            n2 = n / 2;
            for (i = 1; i < n - 1; i++)
            {
                n1 = n2;
                while (j >= n1)
                {
                    j = j - n1;
                    n1 = n1 / 2;
                }
                j = j + n1;

                if (i < j)
                {
                    t1 = x[i];
                    x[i] = x[j];
                    x[j] = t1;
                    t1 = y[i];
                    y[i] = y[j];
                    y[j] = t1;
                }
            }

            // FFT
            n1 = 0;
            n2 = 1;

            for (i = 0; i < m; i++)
            {
                n1 = n2;
                n2 = n2 + n2;
                a = 0;

                for (j = 0; j < n1; j++)
                {
                    c = cos[a];
                    s = sin[a];
                    a += 1 << (m - i - 1);

                    for (k = j; k < n; k = k + n2)
                    {
                        t1 = c * x[k + n1] - s * y[k + n1];
                        t2 = s * x[k + n1] + c * y[k + n1];
                        x[k + n1] = x[k] - t1;
                        y[k + n1] = y[k] - t2;
                        x[k] = x[k] + t1;
                        y[k] = y[k] + t2;
                    }
                }
            }
        }


    }
}
